#include "kf.h"
#include <iostream>
#include <vector>
#include <random>
#include "common_tool/matplotlibcpp.hpp"
#include <cmath>
#include "continuous_linear_system.h"
namespace plt = matplotlibcpp;
using namespace KalmanFilter;
//KF(std::shared_ptr<DiscreteLinearSys> dis_ptr, Eigen::MatrixXd sys_covariance, Eigen::MatrixXd measure_covariance);
int main()
{
	double sample_time = 0.01;
	double s_std = 0.05;
	double v_std = 0.05;
	double y_std = 0.05;
	Eigen::MatrixXd sys_covariance(2, 2);
	sys_covariance << std::pow(s_std, 2), 0,
		0, std::pow(v_std, 2);
	Eigen::MatrixXd measure_covariance(1, 1);
	measure_covariance << 0.1;
	ContinuousLinearSys(2, 1, 0, 1);
	Eigen::MatrixXd state_matrix(2, 2);
	state_matrix << 0, 1,
		0, 0;
	Eigen::MatrixXd control_matrix(2, 1);
	control_matrix << 0,
		1;
	Eigen::MatrixXd out_state_matrix(1, 2);
	out_state_matrix << 1, 0;

	ContinuousLinearSys conti_sys(2, 1, 0, 1);
	conti_sys.set_state_matrix(state_matrix);
	conti_sys.set_control_matrix(control_matrix);
	conti_sys.set_out_state_matrix(out_state_matrix);

	std::shared_ptr<DiscreteLinearSys> test_dis_ptr = conti_sys.discretization(sample_time, "tustin");

	KF test_kf(test_dis_ptr, sys_covariance, measure_covariance);
	//bool reset(Eigen::VectorXd VectorXd, Eigen::MatrixXd state_cov_esti);
	Eigen::VectorXd state_esti(2);
	state_esti << 1, 0.01;
	Eigen::MatrixXd state_cov_esti(2, 2);
	state_cov_esti << 0.01, 0,
		0, 0.01;
	test_kf.reset(state_esti, state_cov_esti);
	std::cout << "state_esti" << test_kf.state_esti() << std::endl;
	std::cout << "state_cov_esti" << test_kf.state_cov_esti() << std::endl;

	std::default_random_engine generator_s;
	std::default_random_engine generator_v;
	std::default_random_engine generator_y;
	std::normal_distribution<double> dist_s(0.0, s_std);
	std::normal_distribution<double> dist_v(0.0, v_std);
	std::normal_distribution<double> dist_y(0.0, y_std);
	Eigen::VectorXd state(2);
	Eigen::VectorXd control(1);
	control << 2.0;
	Eigen::VectorXd output(1);
	std::vector<double> sim_s_v;
	std::vector<double> sim_v_v;
	std::vector<double> s_esti_v;
	std::vector<double> v_esti_v;
	unsigned sim_step = 300;
	for (unsigned int iter = 0; iter < sim_step; iter++)
	{
		state = test_dis_ptr->state_matrix() * state + test_dis_ptr->control_matrix() * control;
		state[0] = state[0] + dist_s(generator_s);
		state[1] = state[1] + dist_v(generator_v);
		sim_v_v.push_back(state[1]);
		output = out_state_matrix * state;
		output[0] = output[0] + dist_y(generator_y);
		sim_s_v.push_back(output[0]);

		//bool step(Eigen::VectorXd &control_last, Eigen::VectorXd &y);
		test_kf.step(control, output);
		Eigen::VectorXd esti_state = test_kf.state_esti();
		s_esti_v.push_back(esti_state[0]);
		v_esti_v.push_back(esti_state[1]);
	}
	plt::plot(sim_s_v);
	plt::plot(s_esti_v, "r--");
	plt::show();
	plt::plot(sim_v_v);
	plt::plot(v_esti_v, "r--");
	plt::show();
}
/*
	double sample_time = 0.01;
	KF test_kf(2, 1, 1);
	//bool init(Eigen::MatrixXd state_matrix, Eigen::MatrixXd control_matrix,
	//		  Eigen::MatrixXd out_state_matrix, Eigen::MatrixXd sys_covariance, Eigen::MatrixXd measure_covariance);
	Eigen::MatrixXd state_matrix(2, 2);
	state_matrix << 1, sample_time,
		0, 1;
	Eigen::MatrixXd control_matrix(2, 1);
	control_matrix << sample_time * sample_time,
		sample_time;
	Eigen::MatrixXd out_state_matrix(1, 2);
	out_state_matrix << 1, 0;
	Eigen::MatrixXd sys_covariance(2, 2);
	sys_covariance << 0.0001, 0,
		0, 0.25;

	Eigen::MatrixXd measure_covariance(1, 1);
	measure_covariance << 0.01;

	test_kf.init(state_matrix, control_matrix, out_state_matrix, sys_covariance, measure_covariance);
	//bool reset(Eigen::VectorXd state_esti, Eigen::MatrixXd state_cov_esti);
	Eigen::VectorXd init_state_esti(2);
	init_state_esti << 0.01, 0.01;
	Eigen::MatrixXd init_state_cov_esti(2, 2);
	init_state_cov_esti << 0.11, 0,
		0, 0.11;
	test_kf.reset(init_state_esti, init_state_cov_esti);
	std::cout << "1st##########################################################" << std::endl;
	std::cout << "state_matrix is:" << std::endl
			  << test_kf.state_matrix() << std::endl;
	std::cout << "control_matrix is:" << std::endl
			  << test_kf.control_matrix() << std::endl;
	std::cout << "out_state_matrix is:" << std::endl
			  << test_kf.out_state_matrix() << std::endl;
	std::cout << "sys_covariance is:" << std::endl
			  << test_kf.sys_covariance() << std::endl;
	std::cout << "measure_covariance is:" << std::endl
			  << test_kf.measure_covariance() << std::endl;
	std::cout << "state_esti is:" << std::endl
			  << test_kf.state_esti() << std::endl;
	std::cout << "state_cov_esti is:" << std::endl
			  << test_kf.state_cov_esti() << std::endl;
	std::cout << "2st########################################################## " << std::endl;
	unsigned int sim_step = 300;

	const double mean = 0.0;   //均值
	const double stddev = 0.1; //标准差
	std::default_random_engine generator_s;
	std::default_random_engine generator_v;
	std::default_random_engine generator_y;
	std::normal_distribution<double> dist_s(mean, sample_time);
	std::normal_distribution<double> dist_v(0.0, 0.5);
	std::normal_distribution<double> dist_y(0.0, 0.1);

	std::vector<double> sim_s_v;
	std::vector<double> sim_v_v;
	Eigen::VectorXd state(2);
	state << 0.0, 0.0;
	Eigen::VectorXd control(1);
	Eigen::VectorXd output(1);
	control << 2.0;
	output << 0.0;
	std::vector<double> s_esti_v;
	std::vector<double> v_esti_v;
	for (unsigned int iter = 0; iter < sim_step; iter++)
	{
		state = state_matrix * state + control_matrix * control;
		state[0] = state[0] + dist_s(generator_s);
		state[1] = state[1] + dist_v(generator_v) + 0.01;

		sim_v_v.push_back(state[1]);
		output = out_state_matrix * state;
		output[0] = output[0] + dist_y(generator_y);
		sim_s_v.push_back(output[0]);

		//bool step(Eigen::VectorXd &control_last, Eigen::VectorXd &y);
		test_kf.step(control, output);
		Eigen::VectorXd esti_state = test_kf.state_esti();
		s_esti_v.push_back(esti_state[0]);
		v_esti_v.push_back(esti_state[1]);
	}
	plt::plot(sim_s_v);
	plt::plot(s_esti_v, "r--");
	plt::show();
	plt::plot(sim_v_v);
	plt::plot(v_esti_v, "r--");
	plt::show();
	*/
